clear;
close all;
clc;

file = load("bun_zipper.txt");
data_source_=file(:,1:3);
for p = 1:size(data_source_(:,1))/6
    data_source(p,:) = data_source_(6*p,:);
end
data_source=data_source';
theta=pi/4;  %旋转角度（此处只有绕z轴旋转）
r = -theta * [0;0;1];
R = rotationVectorToMatrix(r);
t=[0.05,0.05,0.05];   %平移向量
[data_target,T0]=rotate(data_source,R,t);  
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%绘制两幅原始图像
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,'b.'),axis equal
hold on;
scatter3(x2,y2,z2,'r.');
hold off;
 
 
T_final=eye(4,4);   %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %初次更新点集（代表粗配准结果）
err=1;
tic;
while(err>0.000001)
    iteration=iteration+1;    %迭代次数
    disp(['迭代次数ieration=',num2str(iteration)]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %利用欧式距离找出对应点集
    k=size(data_target,2);
    for i = 1:k
        data_q1(1,:) = data_source(1,:) - data_target(1,i);    % 两个点集中的点x坐标之差
        data_q1(2,:) = data_source(2,:) - data_target(2,i);    % 两个点集中的点y坐标之差
        data_q1(3,:) = data_source(3,:) - data_target(3,i);    % 两个点集中的点z坐标之差
        distance = data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2;  % 欧氏距离
        [min_dis, min_index] = min(distance);   % 找到距离最小的那个点
        data_mid(:,i) = data_source(:,min_index);   % 将那个点保存为对应点
        error(i) = min_dis;     % 保存距离差值
    end
    error_mean = mean(error);
    fprintf("mean error=%f, max error=%f, min error=%f\n", error_mean, max(error), min(error));
    for i = 1:k
        w(i)=1;
        %{
       if error(i) > 2 * error_mean
           w(i) = 0;
       else
           w(i) = 1;
       end
        %}
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %去中心化
    data_target_mean=mean(data_target.*w,2);
    data_mid_mean=mean(data_mid.*w,2);
    data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));
    data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));
     
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %SVD分解
    W=zeros(3,3);
    for j=1:size(data_target_c,2)
        W=W+w(j)*data_mid_c(:,j)*data_target_c(:,j)';
    end
    [U,S,V]=svd(W);
    Rf=U*V';
    Tf=data_mid_mean-Rf*data_target_mean;
    err=mean(error);
    T_t=[Rf,Tf];
    T_t=[T_t;0,0,0,1];
    T_final=T_t*T_final;   %更新旋转矩阵
    disp(['误差err=',num2str(err)]);
    disp('旋转矩阵T=');
    disp(T_final);
    disp('U = ');
    disp(U);
    disp('V = ');
    disp(V);
    disp('W = ');
    disp(W);
    data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %更新点集
    if iteration>=200
        break
    end
end
toc;
fprintf("\n");
disp(inv([T0;0,0,0,1]));  %旋转矩阵真值
 
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,'b.'), axis equal
hold on;
scatter3(x2,y2,z2,'r.');
hold off;